{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\left(p_{T x} - p_{x}\\right) \\cos{\\left(\\theta \\right)}}{\\sqrt{\\left|{p_{T x} - p_{x}}\\right|^{2} + \\left|{p_{T y} - p_{y}}\\right|^{2} + \\left|{p_{T z} - p_{z}}\\right|^{2}}} + \\frac{\\left(p_{T y} - p_{y}\\right) \\sin{\\left(\\theta \\right)}}{\\sqrt{\\left|{p_{T x} - p_{x}}\\right|^{2} + \\left|{p_{T y} - p_{y}}\\right|^{2} + \\left|{p_{T z} - p_{z}}\\right|^{2}}}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([[(p_T_x - p_x)*cos(theta)/sqrt(Abs(p_T_x - p_x)**2 + Abs(p_T_y - p_y)**2 + Abs(p_T_z - p_z)**2) + (p_T_y - p_y)*sin(theta)/sqrt(Abs(p_T_x - p_x)**2 + Abs(p_T_y - p_y)**2 + Abs(p_T_z - p_z)**2)]])"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import sympy as sy\n",
    "import numpy as np\n",
    "\n",
    "\n",
    "def transformation_matrix_dh(theta,alpha,a_,d_):\n",
    "\n",
    "    \n",
    "    st=sy.sin(theta)\n",
    "    ct=sy.cos(theta)\n",
    "    sa=sy.sin(alpha)\n",
    "    ca=sy.cos(alpha)\n",
    "    \n",
    "    a=sy.Symbol(a_)\n",
    "    d=sy.Symbol(d_)\n",
    "    \n",
    "    T=sy.Matrix(4,4,[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0])\n",
    "    \n",
    "    T[0,0]=ct\n",
    "    T[1,1]=ct*ca\n",
    "    T[2,2]=ca\n",
    "\n",
    "    T[0,1]=-st*ca\n",
    "    T[2,0]=0\n",
    "    T[1,2]=-ct*sa\n",
    "\n",
    "    T[0,2]=st*sa\n",
    "    T[1,0]=st\n",
    "    T[2,1]=sa\n",
    "\n",
    "    T[0,3]=a*ct\n",
    "    T[1,3]=a*st\n",
    "    T[2,3]=d\n",
    "    T[3,3]=1\n",
    "    \n",
    "    return T\n",
    "\n",
    "\n",
    "def Roderiguez(v1,v2):\n",
    "    \n",
    "    v1n=v1.normalized()\n",
    "    v2n=v2.normalized()\n",
    "    \n",
    "    k=v1n.cross(v2n).normalized()\n",
    "    ct=(v1n.transpose()*v2n)[0]\n",
    "    \n",
    "    st=sy.sqrt(1-ct*ct)\n",
    "    kx=k[0]\n",
    "    ky=k[1]\n",
    "    kz=k[2]\n",
    "    \n",
    "    k_as=sy.Matrix(3,3,[0,-kz,ky,kz,0,-kx,-ky,kx,0])\n",
    "    \n",
    "    I=sy.Matrix(3,3,np.eye(3).flatten())\n",
    "    \n",
    "    R=I*ct+(1-ct)*k*(k.transpose())+st*k_as\n",
    "    \n",
    "    \n",
    "    return R\n",
    "\n",
    "trans=transformation_matrix_dh(\"theta\",\"alpha\",\"a\",\"d\").subs('alpha',0)\n",
    "R=trans[:3,:3]\n",
    "\n",
    "unitX=sy.Matrix(3,1,[1,0,0])\n",
    "vc=R*unitX # camera x-axis vector in world frame\n",
    "\n",
    "px,py,pz=sy.symbols(['p_x','p_y','p_z'])\n",
    "ptx,pty,ptz=sy.symbols(['p_T_x','p_T_y','p_T_z'])\n",
    "pc=sy.Matrix(3,1,[px,py,pz])\n",
    "pt=sy.Matrix(3,1,[ptx,pty,ptz])\n",
    "vtc=pt-pc #3d vector of target in camera frame\n",
    "\n",
    "\n",
    "\n",
    "R_t_cam=Roderiguez(vc,vtc) #rotation bewteen camera x-axis and target\n",
    "\n",
    "\n",
    "(vc.transpose()*vtc.normalized())\n",
    "\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2.7.12 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "2.7.12"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "767d51c1340bd893661ea55ea3124f6de3c7a262a8b4abca0554b478b1e2ff90"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
